#include "MPU6050.h"

I2C_TypeDef *MPU6050_I2Cx = I2C2;
int16_t GYROZ_0 = 0;
uint16_t busytime = 0;

#define SB (IS_BIT_1(MPU6050_I2Cx->SR1, BIT(0)))
#define EV5 I2C_EVENT_MASTER_MODE_SELECT
#define ADDR (IS_BIT_1(MPU6050_I2Cx->SR1, BIT(1)))
#define EV6 I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED
#define EV7 I2C_EVENT_MASTER_BYTE_RECEIVED
#define EV8 I2C_EVENT_MASTER_BYTE_TRANSMITTING
#define EV8_2 I2C_EVENT_MASTER_BYTE_TRANSMITTED

void MPU6050_W_SCL(uint8_t BitValue)
{
    GPIO_WriteBit(GPIOB, GPIO_Pin_10, (BitAction)BitValue);
    Delay_us(10);
}

void MPU6050_W_SDA(uint8_t BitValue)
{
    GPIO_WriteBit(GPIOB, GPIO_Pin_11, (BitAction)BitValue);
    Delay_us(10);
}

uint8_t MPU6050_R_SDA(void)
{
    uint8_t BitValue;
    BitValue = GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_11);
    Delay_us(10);
    return BitValue;
}

void MPU6050_I2C_start(void)
{
    MPU6050_W_SDA(1);
    MPU6050_W_SCL(1);
    MPU6050_W_SDA(0);
    MPU6050_W_SCL(0);
}

void MPU6050_I2C_stop(void)
{
    MPU6050_W_SDA(0);
    MPU6050_W_SCL(1);
    MPU6050_W_SDA(1);
}

void MPU6050_I2C_sendByte(uint8_t byte)
{
    uint8_t i;
    for (i = 0; i < 8; i++)
    {
        MPU6050_W_SDA(byte & (0x80 >> i));
        MPU6050_W_SCL(1);
        MPU6050_W_SCL(0);
    }
}

uint8_t MPU6050_I2C_receiveByte()
{
    uint8_t i, byte = 0x00;
    MPU6050_W_SDA(1);
    for (i = 0; i < 8; i++)
    {
        MPU6050_W_SCL(1);
        if (MPU6050_R_SDA() == 1)
        {
            byte |= (0x80 >> i);
        }
        MPU6050_W_SCL(0);
    }
    return byte;
}

void MPU6050_I2C_sendAck(uint8_t AckBit)
{
    MPU6050_W_SDA(AckBit);
    MPU6050_W_SCL(1);
    MPU6050_W_SCL(0);
}

uint8_t MPU6050_I2C_receiveAck(void)
{
    uint8_t AckBit;
    MPU6050_W_SDA(1);
    MPU6050_W_SCL(1);
    AckBit = MPU6050_R_SDA();
    MPU6050_W_SCL(0);
    return AckBit;
}

/**
 * @brief  OLED写命令
 * @param  Command 要写入的命令
 * @retval 无
 */
void MPU6050_writeReg(uint8_t reg, uint8_t Data)
{
    MPU6050_I2C_start();
    MPU6050_I2C_sendByte(MPU6050_SLAVE_ADDRESS_0);
    MPU6050_I2C_receiveAck();
    MPU6050_I2C_sendByte(reg);
    MPU6050_I2C_receiveAck();
    MPU6050_I2C_sendByte(Data);
    MPU6050_I2C_receiveAck();
    MPU6050_I2C_stop();
}

uint8_t MPU6050_readReg(uint8_t reg)
{
    uint8_t Data;

    MPU6050_I2C_start();
    MPU6050_I2C_sendByte(MPU6050_SLAVE_ADDRESS_0);
    MPU6050_I2C_receiveAck();
    MPU6050_I2C_sendByte(reg);
    MPU6050_I2C_receiveAck();

    MPU6050_I2C_start();
    MPU6050_I2C_sendByte(MPU6050_SLAVE_ADDRESS_0 | 0x01);
    MPU6050_I2C_receiveAck();
    Data = MPU6050_I2C_receiveByte();
    MPU6050_I2C_sendAck(1);
    MPU6050_I2C_stop();

    return Data;
}

/**
 * @brief 从寄存器中读入16位数据
 *
 * @param reg 8位地址
 * @return uint16_t 16位数据
 */
uint16_t MPU6050_readWord(uint8_t reg)
{
    uint8_t Data_H = 2;
    uint8_t Data_L = 2;

    Data_H = MPU6050_readReg(reg);
    Data_L = MPU6050_readReg(reg + 1);

    return (Data_H << 8) | Data_L;
}

void MPU6050_init()
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    MPU6050_regInit();
}

void MPU6050_regInit()
{
    MPU6050_writeReg(PWR_MGMT_1, 0x80); // 复位
    Delay_ms(100);
    MPU6050_writeReg(PWR_MGMT_1, 0x00); // 唤醒
    Delay_ms(100);
    MPU6050_writeReg(PWR_MGMT_2, 0x00);   // 关闭待机
    MPU6050_writeReg(INT_ENABLE, 0x00);   // 关闭中断
    MPU6050_writeReg(CONFIG, 0x04);       // 设置低通滤波器
    MPU6050_writeReg(GYRO_CONFIG, 0x18);  // 陀螺仪满量程
    MPU6050_writeReg(ACCEL_CONFIG, 0x18); // 加速度满量程
    MPU6050_writeReg(INT_PIN_CFG, 0x02);
    MPU6050_writeReg(USER_CTRL, 0x00);
}

void MPU6050_initGyro()
{
    uint16_t i;
    GYROZ_0 = 0;
    for (i = 0; i < 5; i++)
    {
        Delay_ms(2);
        GYROZ_0 += (int16_t)(MPU6050_readWord(GYRO_ZOUT)) / 5;
    }
}

void MPU6050_setGyroRange(uint8_t MPU6050_RANGE)
{
    uint8_t temp = 0;
    temp = MPU6050_readReg(GYRO_CONFIG);
    temp |= (MPU6050_RANGE << 3);
    MPU6050_writeReg(GYRO_CONFIG, temp);
}

void MPU6050_setAccelRange(uint8_t MPU6050_RANGE)
{
    uint8_t temp = 0;
    temp = MPU6050_readReg(ACCEL_CONFIG);
    temp |= (MPU6050_RANGE << 3);
    MPU6050_writeReg(ACCEL_CONFIG, temp);
}

void setFilterBandwidth(uint8_t MPU6050_BAND)
{
    uint8_t temp = 0;
    temp = MPU6050_readReg(CONFIG);
    temp |= MPU6050_BAND;
    MPU6050_writeReg(CONFIG, temp);
}

int16_t MPU6050_getGyro()
{
    double gy16 = (int16_t)MPU6050_readWord(GYRO_ZOUT);
    return ROUND_TO_INT16(2000 * (gy16 - GYROZ_0) / 32768);
}
